#ifndef __PID_KIT_PIDCONTROLLER_H__
#define __PID_KIT_PIDCONTROLLER_H__
#pragma once

#include <limits>
#include <type_traits>

namespace pidkit
{
    template <typename T, typename U = T>
    class PIDController
    {
        static_assert(std::is_floating_point<T>::value, "PID parameters must be floating point type");
        static_assert(std::is_arithmetic<U>::value, "Input/Output type must be arithmetic type");

    public:
        using pid_param_type = T; // 参数类型
        using in_out_type = U;    // 输入输出类型

        PIDController() = default;

        /// @brief 构造函数
        /// @param kp 比例参数（浮点类型）
        /// @param ki 积分参数（浮点类型）
        /// @param kd 微分参数（浮点类型）
        /// @param dt 默认时间间隔（浮点类型）
        /// @param output_min 输出最小值
        /// @param output_max 输出最大值
        PIDController(T kp, T ki, T kd, T dt = T(0.001),
                      U output_min = std::numeric_limits<U>::lowest(),
                      U output_max = std::numeric_limits<U>::max())
            : __kp(kp), __ki(ki), __kd(kd), __dt(dt),
              __output_min(output_min), __output_max(output_max) {}

        /// @brief 设置PID参数
        /// @param kp 比例参数（浮点类型）
        /// @param ki 积分参数（浮点类型）
        /// @param kd 微分参数（浮点类型）
        void set_parameters(T kp, T ki, T kd)
        {
            __kp = kp;
            __ki = ki;
            __kd = kd;
        }

        /// @brief 设置默认时间间隔
        /// @param dt 时间间隔（浮点类型）
        void set_dt(T dt)
        {
            __dt = dt;
        }

        /// @brief 获取默认时间间隔
        /// @return 当前默认时间间隔
        T get_dt() const { return __dt; }

        /// @brief 单独设置P参数
        /// @param kp 比例参数（浮点类型）
        void set_p(T kp)
        {
            __kp = kp;
        }

        /// @brief 单独设置I参数
        /// @param ki 积分参数（浮点类型）
        void set_i(T ki)
        {
            __ki = ki;
        }

        /// @brief 单独设置D参数
        /// @param kd 微分参数（浮点类型）
        void set_d(T kd)
        {
            __kd = kd;
        }

        /// @brief 设置输出限制
        /// @param min 输出最小值
        /// @param max 输出最大值
        void set_output_limits(U min, U max)
        {
            __output_min = min;
            __output_max = max;
        }

        /// @brief 设置积分限制
        /// @param limit 积分限幅值（浮点类型）
        void set_integral_limit(T limit)
        {
            __integral_limit = limit;
        }

        /// @brief 重置控制器状态
        void reset()
        {
            __last_error = T(0);
            __integral = T(0);
        }

        /// @brief 计算PID输出（使用默认时间间隔）
        /// @param setpoint 目标值
        /// @param measurement 测量值
        /// @return 计算得到的控制输出
        U compute(U setpoint, U measurement)
        {
            return compute(setpoint, measurement, __dt);
        }

        /// @brief 计算PID输出（指定时间间隔）
        /// @param setpoint 目标值
        /// @param measurement 测量值
        /// @param dt 时间间隔（浮点类型）
        /// @return 计算得到的控制输出
        U compute(U setpoint, U measurement, T dt)
        {
            // 计算误差（转换为浮点类型进行计算）
            T error = static_cast<T>(setpoint) - static_cast<T>(measurement);

            // 计算积分项
            __integral += error * dt;

            // 积分限幅
            if (__integral_limit > T(0))
            {
                if (__integral > __integral_limit)
                {
                    __integral = __integral_limit;
                }
                else if (__integral < -__integral_limit)
                {
                    __integral = -__integral_limit;
                }
            }

            // 计算微分项（使用误差的微分）
            T derivative = (error - __last_error) / dt;

            // 计算PID输出（使用浮点类型计算）
            T output = __kp * error + __ki * __integral + __kd * derivative;

            // 输出限幅（转换为目标类型）
            if (output > static_cast<T>(__output_max))
            {
                output = static_cast<T>(__output_max);
            }
            else if (output < static_cast<T>(__output_min))
            {
                output = static_cast<T>(__output_min);
            }

            // 更新状态
            __last_error = error;

            // 返回转换后的输出值
            return static_cast<U>(output);
        }

        /// @brief 获取当前P参数
        /// @return 比例参数（浮点类型）
        T get_kp() const { return __kp; }

        /// @brief 获取当前I参数
        /// @return 积分参数（浮点类型）
        T get_ki() const { return __ki; }

        /// @brief 获取当前D参数
        /// @return 微分参数（浮点类型）
        T get_kd() const { return __kd; }

        /// @brief 获取当前积分值
        /// @return 积分值（浮点类型）
        T get_integral() const { return __integral; }

        /// @brief 获取上次误差
        /// @return 上次误差（浮点类型）
        T get_last_error() const { return __last_error; }

    private:
        T __kp{T(0)};                                     // 比例系数（浮点类型）
        T __ki{T(0)};                                     // 积分系数（浮点类型）
        T __kd{T(0)};                                     // 微分系数（浮点类型）
        T __dt{T(0.001)};                                 // 默认时间间隔（浮点类型）
        T __integral{T(0)};                               // 积分项（浮点类型）
        T __last_error{T(0)};                             // 上次误差（浮点类型）
        U __output_min{std::numeric_limits<U>::lowest()}; // 输出最小值
        U __output_max{std::numeric_limits<U>::max()};    // 输出最大值
        T __integral_limit{T(0)};                         // 积分限幅值（浮点类型）
    };
} // namespace pidkit
#endif // __PID_KIT_PIDCONTROLLER_H__